/* INCLUSIONS */
#include "SERIALAPI.h"

/* GLOBAL VARIABLES */
int8_t M1_SPEED = 0x00;
int8_t M2_SPEED = 0x00;
TIM_MATCHCFG_Type MCFG;
uint8_t TURNING = 0x00;

/* FUNCTION BODIES */
void INITIALISE(void) {
  UART_CFG_Type      UARTConfigStruct     ;
  UART_FIFO_CFG_Type UARTFIFOConfigStruct ;
  PINSEL_CFG_Type    PinCfg               ;

  UART_ConfigStructInit(&UARTConfigStruct)       ;
  UARTConfigStruct.Baud_rate = (uint32_t) 115200 ;

  UART_FIFOConfigStructInit (&UARTFIFOConfigStruct)                                ;
  UART_Init                 ((LPC_UART_TypeDef *)LPC_UART3, &UARTConfigStruct)     ;
  UART_FIFOConfig           ((LPC_UART_TypeDef *)LPC_UART3, &UARTFIFOConfigStruct) ;
  UART_TxCmd                ((LPC_UART_TypeDef *)LPC_UART3, ENABLE)                ;

  PinCfg.Funcnum   = 2      ;
  PinCfg.OpenDrain = 0      ;
  PinCfg.Pinmode   = 0      ;
  PinCfg.Portnum   = 0      ;
  PinCfg.Pinnum    = 0      ;
  PINSEL_ConfigPin(&PinCfg) ;
  PinCfg.Pinnum    = 1      ;
  PINSEL_ConfigPin(&PinCfg) ;

  UART_ConfigStructInit(&UARTConfigStruct)       ;
  UARTConfigStruct.Baud_rate = (uint32_t) 115200 ;

  UART_FIFOConfigStructInit (&UARTFIFOConfigStruct)                                ;
  UART_Init                 ((LPC_UART_TypeDef *)LPC_UART3, &UARTConfigStruct)     ;
  UART_FIFOConfig           ((LPC_UART_TypeDef *)LPC_UART3, &UARTFIFOConfigStruct) ;
  UART_TxCmd                ((LPC_UART_TypeDef *)LPC_UART3, ENABLE)                ;
  
  TIMER_INIT(LPC_TIM0, 1000);
  TIMER_MATCH_CFG(&MCFG, LPC_TIM0, 1);
}


void MOVE_FORWARD (uint8_t SPEED) {
  uint8_t CMD_DATA[4] = {M1FBYTE, SPEED, M2FBYTE, SPEED}          ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING);
  M1_SPEED = SPEED;
  M2_SPEED = SPEED;
  _DBG_("M1 SPEED: ");
  _DBH((uint8_t) M1_SPEED);
  _DBG_("\n");
  _DBG_("M2 SPEED: ");
  _DBH((uint8_t) M2_SPEED);
  _DBG_("\n");
}

void MOVE_REVERSE (uint8_t SPEED) {
  uint8_t CMD_DATA[4] = {M1RBYTE, SPEED, M2RBYTE, SPEED}           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING) ;
  M1_SPEED = (0xFF - SPEED);
  M2_SPEED = (0xFF - SPEED);
  _DBG_("M1 SPEED: ");
  _DBH((uint8_t) M1_SPEED);
  _DBG_("\n");
  _DBG_("M2 SPEED: ");
  _DBH((uint8_t) M2_SPEED);
  _DBG_("\n");
}

void STOP_MOTORS (void) {
  uint8_t CMD_DATA[4] = {M1FBYTE, 0x00, M2FBYTE, 0x00};
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING) ;
  M1_SPEED = 0x00;
  M2_SPEED = 0x00;
  _DBG_("M1 SPEED: ");
  _DBH((uint8_t) M1_SPEED);
  _DBG_("\n");
  _DBG_("M2 SPEED: ");
  _DBH((uint8_t) M2_SPEED);
  _DBG_("\n");
}

void M1_STOP(void) {
  uint8_t CMD_DATA[2] = {M1FBYTE, 0x00};
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M1_SPEED = 0x00;
  _DBG_("M1 SPEED: ");
  _DBH((uint8_t) M1_SPEED);
  _DBG_("\n");
}

void M2_STOP(void) {
  uint8_t CMD_DATA[2] = {M2FBYTE, 0x00};
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M2_SPEED = 0x00;
  _DBG_("M2 SPEED: ");
  _DBH((uint8_t) M2_SPEED);
  _DBG_("\n");
}

void M1_FORWARD (uint8_t SPEED) {
  uint8_t CMD_DATA[2] = {M1FBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M1_SPEED = SPEED;
  _DBG_("M1 SPEED: ");
  _DBH((uint8_t) M1_SPEED);
  _DBG_("\n");
}

void M1_REVERSE (uint8_t SPEED) {
  uint8_t CMD_DATA[2] = {M1RBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M1_SPEED = -SPEED;
  _DBG_("M1 SPEED: ");
  _DBH((uint8_t) M1_SPEED);
  _DBG_("\n");
}

void M2_FORWARD (uint8_t SPEED) {
  uint8_t CMD_DATA[2] = {M2FBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M2_SPEED = SPEED;
  _DBG_("M2 SPEED: ");
  _DBH((uint8_t) M2_SPEED);
  _DBG_("\n");
}

void M2_REVERSE (uint8_t SPEED) {
  uint8_t CMD_DATA[2] = {M2RBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M2_SPEED = -SPEED;
  _DBG_("M2 SPEED: ");
  _DBH((uint8_t) M2_SPEED);
  _DBG_("\n");
}

void M1_INCREASE (uint8_t SPEED) {
  if ((M1_SPEED + SPEED) >= 0) {
    M1_FORWARD(M1_SPEED + SPEED);
  }
  else {
    M1_REVERSE(abs(M1_SPEED - SPEED));
  }
}

void M1_DECREASE (uint8_t SPEED) {
  if ((M1_SPEED - SPEED) <= 0) {
    M1_REVERSE(SPEED - M1_SPEED);
  }
  else {
    M1_FORWARD(M1_SPEED - SPEED);
  }
}

void M2_INCREASE (uint8_t SPEED) {
  if ((M2_SPEED + SPEED) >= 0) {
    M2_FORWARD(M2_SPEED + SPEED);
  }
  else {
    M2_REVERSE(abs(M2_SPEED - SPEED));
  }
}

void M2_DECREASE (uint8_t SPEED) {
  if ((M2_SPEED - SPEED) <= 0) {
    M2_REVERSE(SPEED - M2_SPEED);
  }
  else {
    M2_FORWARD(M2_SPEED - SPEED);
  }
}

void ROTATE_CLKW(uint32_t DEGREES) {
  _DBG_("FLAG STATUS:\n");
  _DBH(TURNING);
  while (TURNING) {_DBG_("WAITING TO TURN CLKW!\n");}
  TURNING = 1;
  _DBG_("TURNING CLKW!\n");
  TIMER_MVALUE_EDIT(&MCFG, DEGREES*(160/18));
  TIMER_ENABLE(LPC_TIM0);
  M1_FORWARD(0x10);
  M2_REVERSE(0x10);
}

void ROTATE_CCLKW(uint32_t DEGREES) {
  _DBG_("FLAG STATUS:\n");
  _DBH(TURNING);
  while (TURNING) {_DBG_("WAITING TO TURN CCLKW!\n");}
  _DBG_("TURNING CCLKW!\n");
  TURNING = 1;
  TIMER_MVALUE_EDIT(&MCFG, DEGREES*(160/18));
  TIMER_ENABLE(LPC_TIM0);
  M1_REVERSE(0x10);
  M2_FORWARD(0x10);
}

void TIMER0_IRQHandler(void) {
  if (TIM_GetIntStatus(LPC_TIM0, TIM_MR0_INT) == SET) {
    _DBG_("ROTATE COMPLETE!\n");
    STOP_MOTORS();
    TIMER_DISABLE(LPC_TIM0);
    TURNING = 0;
  }
  TIM_ClearIntPending(LPC_TIM0, TIM_MR0_INT);
}
